#ifndef __BOOLEAN_MESH_
#define __BOOLEAN_MESH_


#include <vector>
#include <map>
#include <deque>
#include <boost/lambda/lambda.hpp>

#include "common.h"
#include "mesh.h"
#include "aabbtree.h"

#include "bm_primitive.h"


// This structure is used for merging intersection. 
extern std::map <std::pair <unsigned, unsigned>, IntersectionRecord<Value> > map_mergeInter;

template <typename BooleanMeshPrimitive>
class BooleanMesh : public Mesh <BooleanMeshPrimitive>
{
public:
	class SegmentStruct ;
public:
	typedef typename BooleanMeshPrimitive::ValueType              ValueType; 
	typedef typename BooleanMeshPrimitive::FaceType               FaceType;
	typedef typename BooleanMeshPrimitive::VertexType             VertexType;
	typedef typename BooleanMeshPrimitive::HalfEdgeType           HalfEdgeType ; 
	typedef typename BooleanMeshPrimitive::EdgeType               EdgeType ; 
	typedef typename BooleanMeshPrimitive::VectorType             VectorType ;
	typedef typename BooleanMeshPrimitive::PointType              PointType ;
	typedef typename BooleanMeshPrimitive::MeshType               MeshType ;
	typedef typename Mesh<BooleanMeshPrimitive>::HalfEdgeIterator HalfEdgeIterator ;
	typedef typename Mesh<BooleanMeshPrimitive>::VertexIterator   VertexIterator ;
	typedef Segment<ValueType>                                    SegmentType ;
    typedef Ray<ValueType>                                        RayType ;
		

	typedef AABBTreePoly <ValueType>                         AABBTreePolyType; 
	typedef AABBTreePolyBuilder <ValueType>                  AABBTreePolyBuilderType  ; 
	typedef AABBTreePolygon <ValueType>                      AABBTreePolygonType ;
	typedef AABBTreeNode <ValueType> AABBTreeNodeType ;
	typedef Box<ValueType> BoxType; 
	typedef Polygon<ValueType> PolygonType; 

    typedef typename AABBTreePolyType::PolygonColResult           PolygonColResult;
    typedef typename AABBTreePolyType::RayColResult               RayColResult;

	typedef typename std::multimap<unsigned, unsigned>::iterator  MapUUIterator ;

public:
	enum PARTTYPE {IN = 0x0001, ON = 0x0002, OUT = 0x0004 }; 
		
public :
	/**
	 * Default constructor. 
	 */
	BooleanMesh (): tree (NULL) {}

	/**
	 * Virtual destructor. 
	 */
	virtual ~ BooleanMesh() 
	{
		if (tree !=NULL)
			delete tree ;
	}

	/**
	 * Mesh constructor. 
	 */
	void construct()
	{
		for (HalfEdgeIterator he_iter  = this->hash_he.begin(); he_iter != this->hash_he.end(); ++he_iter)
		{
			he_iter->second->mp_mesh  = (MeshType *) (this) ;
		}

		for (VertexIterator v_iter  = this->vertices.begin(); v_iter != this->vertices.end(); ++v_iter)
		{
			v_iter->second->mp_mesh  = (MeshType *) (this) ;
		}
	}

	/**
	 * Build aabbtree from the polygon in mesh. 
	 * @post The coordinates of vertices cannot be changed. Otherwise, the tree must be rebuilt. 
	 */
	AABBTreePolyType * build_aabbtree() 
	{
		if (tree != NULL) delete tree; 
		AABBTreePolyBuilderType builder  ; 
		this->tree = builder.build_from_mesh ((Mesh<MeshPrimitive<ValueType> > &)(*this) ,1);
		return this->tree; 
	}

	/**
	 * Get the aabbtree pointer. 
	 * @return the aabbtree pointer. 
	 */
	AABBTreePolyType * get_aabbtree () const 
	{
		return this->tree; 
	}

	/**
	 * test whether a point is in or out of the B-rep. 
	 * Now we dont reply to on the boundary. 
	 * @param _p the point to query
	 * @param _normal the direction of the testing ray. 
	 - Actually, we should sort the polygon intersected. Here we only pick on face and compare with its normal for simplicity. 
	*/



    /**
     *
     */
	FaceType * construct_face2 (MeshType & mesh, const std::vector <unsigned > & v_id) 
	{
		unsigned v_size = v_id.size(); 
	 	FaceType * p_face = new FaceType; 
		std::vector<HalfEdgeType * > v_he; 
		for (unsigned i = 0; i < v_size; ++ i)
		{
			v_he.push_back(new HalfEdgeType);
		}

		for (unsigned i = 0; i < v_size; ++i)
		{
			v_he[i]->face = p_face ;
			v_he[i]->prev = v_he[(i + v_size - 1) % v_size] ;
			v_he[i]->next = v_he[(i + 1) % v_size] ;
			
			v_he[i]->from_v = mesh.vertices[v_id [i]];
			mesh.vertices[v_id[i]]->he = v_he[i] ;

			mesh.hash_he [HashKey (v_id[i], v_id[(i + 1 ) % v_size])] = v_he[i] ;
		}

		p_face->he = v_he[0] ;
		p_face->m_idx =  this->faces.size();
		mesh.faces.push_back(p_face);


		// Update the flips and Edges 
		for (unsigned i = 0; i < v_size; ++i)
		{
			unsigned id1 = v_id[i];
			unsigned id2 = v_id[ (i + 1) % v_size] ;

			HalfEdgeIterator iter ;

			iter = mesh.hash_he.find(HashKey(id2, id1)) ;
			if (mesh.hash_he.end() == iter)
			{
				v_he[i]->flip = NULL;
				EdgeType * edge = new EdgeType;
				edge->he = v_he[i] ;
				v_he[i]->edge = edge;
				if (id1 < id2) mesh.edges[HashKey(id1, id2)] = edge;
				else mesh.edges[HashKey(id2, id1)] = edge;
			}
			else
			{
				v_he[i]->flip = (*iter).second;
				(*iter).second->flip = v_he[i] ;
				v_he[i]->edge = (*iter).second->edge ;
			}
			
		}

		p_face->construct(); 
		return p_face; 
	}

	/**
	 * create the new face for new mesh. 
	 * @param new_mesh The reference of new mesh. 
	 * @param v_he Vector of the old face in counterclock wise order. 
	 * @param vertex_map The map from the Old vertex point to  the  NEW vertex pointer in pairwise. 
	 */
	void copy_face (MeshType & new_mesh, const std::vector<HalfEdgeType *> & v_he, std::map <VertexType *, VertexType * > & vertex_map, std::map <VertexType *, VertexType * > & boundary_vertex_map, VectorType & normal) 
	{
		static unsigned vertex_id = 0; 
		std::vector<unsigned> v_vertex_id ;
		// Step 1. Create New Vertex. If the Vertex Exist
		for (unsigned i = 0; i < v_he.size(); ++i)
		{


			VertexType * old_vertex = (VertexType *)v_he[i]->from_v ;
			// Substitute the boundary vertex. 
			typename std::map <VertexType *, VertexType * >::iterator boundary_iter_pos = boundary_vertex_map.find (old_vertex);
			if ( boundary_iter_pos != boundary_vertex_map.end())
			{
				//std::cout<<"FIND"<<std::endl;
				old_vertex = boundary_iter_pos->second ;
			}

			typename std::map <VertexType *, VertexType * >::iterator iter_pos = vertex_map.find (old_vertex) ;
			if ( iter_pos == vertex_map.end())
			{
				VertexType * pv = new VertexType ;
				pv->m_coord = old_vertex->m_coord ;
				pv->m_idx = vertex_id++ ;
				new_mesh.vertices[pv->m_idx] = pv ;
				v_vertex_id.push_back(pv->m_idx);

				vertex_map.insert(std::make_pair(old_vertex, pv));
			}
			else
			{
				v_vertex_id.push_back(iter_pos->second->m_idx);
			}
		}
		for (unsigned i = 0 ; i < v_vertex_id.size(); ++i)
		{
			std::cout<<v_vertex_id[i]<<' ';
		}
		std::cout<<std::endl;
		// Step 2. Construct the face 
		FaceType * p_face = this->construct_face2(new_mesh, v_vertex_id); 
		// Step 3. Set face normal 
		p_face->normalVector = normal;
	}

	/**
	 * Copy mesh by traversing halfedges only. 
	 * @new_mesh The reference of the new empty mesh. 
	 */
	void copy_mesh (MeshType & mesh1, MeshType & mesh2, FaceType * begin_face , MeshType & new_mesh, 	std::map<unsigned, unsigned > & vv_relation) 
	{
		// build boundary vertex_map; 
		std::map <VertexType *, VertexType * > boundary_vertex_map ;
		typename std::map <std::pair <unsigned, unsigned>, IntersectionRecord<Value> >::iterator iter1 ;
		typename std::map <std::pair <unsigned, unsigned>, IntersectionRecord<Value> >::iterator iter2 ;

		typename std::map<unsigned, unsigned>::iterator vv_iter ;
		for (vv_iter = vv_relation.begin(); vv_iter !=vv_relation.end(); ++vv_iter )
		{
			boundary_vertex_map.insert (std::make_pair (mesh1.vertices[vv_iter->first], mesh2.vertices[vv_iter->second]));
		}
/*
		for (iter1 = mesh1.map_mergeInter.begin(); iter1 != mesh1.map_mergeInter.end(); ++iter1)
		{
			std::pair<unsigned, unsigned> key2 = iter1->first ;
			swap (key2.first, key2.second);
			HalfEdgeType * he1 = iter1->second.v_merge_he[0].first ;
			iter2 = mesh2.map_mergeInter.find (key2);
			if (iter2 == mesh2.map_mergeInter.end()) throw ("Find corresponding key failed.");
			HalfEdgeType * he2 = iter2->second.v_merge_he[0].first ;

			boundary_vertex_map.insert (std::make_pair((VertexType *)he1->from_v , (VertexType *)he2->next->from_v ));
			boundary_vertex_map.insert (std::make_pair((VertexType *)he1->next->from_v, (VertexType *)he2->from_v));
		}
*/		

		std::deque<FaceType * > face_deque ; 
		std::map <VertexType *, VertexType * > vertex_map ;

		// Actually, we have to specifiy a correct starting traversing face. 
		//FaceType * begin_face = (FaceType *) this->faces[0]; 
		face_deque.push_back (begin_face);
		begin_face->accessed = true; 
		unsigned count = 0; 
		while (face_deque.size())
		{
			FaceType * face = face_deque[0] ;
			face_deque.pop_front();

			std::vector<HalfEdgeType *> v_he ;
			face->Face2Halfedge(v_he);

			// Create new face here
			count ++ ;
			copy_face (new_mesh, v_he, vertex_map, boundary_vertex_map, face->normalVector) ;

			for (unsigned i = 0; i < v_he.size(); ++i)
			{
				if (v_he[i]->flip != NULL && ((FaceType *) (v_he[i]->flip->face))->accessed != true)
				{
					((FaceType * )v_he[i]->flip->face)->accessed = true; 
					face_deque.push_back((FaceType * )v_he[i]->flip->face);
				}
			}
		}
		std::cout<<"Traverse count "<<count<<std::endl;
		
	}

	
	
private :

	int add_vertex (HalfEdgeType * he)
	{
		unsigned start_idx = this->vertices.size(); 
		
		if (false == he->is_allocated())
		{
			VertexType * pv = new VertexType ;
			this->vertices[start_idx] = pv; 
			pv->m_coord = he->m_intr_point ;
			he->set_point_idx(start_idx);
			return start_idx; 
		}
		else
		{
			return he->get_point_idx();
		}		
	}



private:
	

public :

	void mesh_intersection_relation (MeshType & mesh2)
	{
		this->tree2 = mesh2.get_aabbtree() ;
		MeshType & mesh1 = *this ;
		typename AABBTreePolyType::PolygonColResult polygonColResult; 

		AABBTreePolyType * tree2 = mesh2.get_aabbtree();

/*
		unsigned mesh1_id = 0 ; // check face
		unsigned mesh2_id = 64;

		const PolygonType & p1 = mesh1.faces[mesh1_id]->get_polygon (); 
		const PolygonType & p2 = mesh2.faces[mesh2_id]->get_polygon(); 
		std::cout<<p1<<std::endl;
		std::cout<<p2<<std::endl;
		bool flag = predicate_polygon_polygon(p1, p2); 
		if (flag)
		{
			this->map_mesh1to2.insert (std::make_pair (mesh1_id, mesh2_id));
			mesh2.map_mesh1to2.insert (std::make_pair (mesh2_id, mesh1_id));
		}
*/
/*
//		unsigned mesh1_id = 317 ; // check face
		unsigned mesh1_id = 1 ; // check face
		//unsigned mesh1_id = 94 ; 
		const PolygonType & p1 = mesh1.faces[mesh1_id]->get_polygon (); 
		//const PolygonType & p2 = mesh2.faces[60]->get_polygon(); 
		//bool flag = predicate_polygon_polygon(p1, p2); 
		
		std::cout<<"P1 "<<p1<<std::endl;
		tree2->collideWithPolygon (p1, polygonColResult);
		if (polygonColResult.polys_.size() > 0)
		{
			v_face_id.insert(mesh1_id);
			for (unsigned i = 0; i < polygonColResult.polys_.size(); ++i)
			{
				unsigned mesh2_id = polygonColResult.polys_[i]->get_face_idx(); 
				this->map_mesh1to2.insert (std::make_pair (mesh1_id, mesh2_id));
				mesh2.map_mesh1to2.insert (std::make_pair (mesh2_id, mesh1_id));
				//this->key_mesh1.insert (mesh1_id);
				//this->key_mesh2.insert (mesh2_id);
			}
		}
		polygonColResult.polys_.clear();
*/

		unsigned size1 = mesh1.faces.size();
		for (unsigned mesh1_id = 0; mesh1_id < size1; ++mesh1_id)
		{		

			const PolygonType & p1 = mesh1.faces[mesh1_id]->get_polygon (); 
			//std::cout<<"mesh1_id "<<mesh1_id<<std::endl;
			tree2->collideWithPolygon (p1, polygonColResult);
			if (polygonColResult.polys_.size() > 0)
			{
				v_face_id.insert(mesh1_id);
				for (unsigned i = 0; i < polygonColResult.polys_.size(); ++i)
				{
					unsigned mesh2_id = polygonColResult.polys_[i]->get_face_idx(); 
					this->map_mesh1to2.insert (std::make_pair (mesh1_id, mesh2_id));
					mesh2.map_mesh1to2.insert (std::make_pair (mesh2_id, mesh1_id));
					//std::cout<<mesh1_id<<' '<<mesh2_id<<std::endl;
					//this->key_mesh1.insert (mesh1_id);
					//this->key_mesh2.insert (mesh2_id);
				}
			}
			polygonColResult.polys_.clear();
		}

	}

	void face_splitter(MapUUIterator begin_iter, MapUUIterator end_iter, MeshType & mesh1, MeshType & mesh2)
	{
		v_segment.clear(); 
		v_halfedge.clear();
		bool coplanar = false; 
		std::vector<std::vector<Vector<Value> > > cut_line_set ;
		std::vector<bool> cut_line_type; // iscoplanar or not ;
		std::vector<std::pair<HalfEdgeType *, HalfEdgeType *> > cut_he_set ;
		FaceType * split_face = NULL;
		unsigned id1, id2;
		for (MapUUIterator in_map_iter = begin_iter; in_map_iter != end_iter; ++ in_map_iter )
		{
			id1 = in_map_iter->first ;
			id2 = in_map_iter->second ;
			std::cout<<"\n\nFace Splitter ["<<id1<<' '<<id2<<"]\n";
			split_face = mesh1.faces[in_map_iter->first];
			//const PolygonType & p1 = this->faces[in_map_iter->first]->get_polygon (); 
			//const PolygonType & p2 = p_mesh2->faces[in_map_iter->second]->get_polygon (); 
			BooleanMeshIntersectResult<ValueType> intrRes; 

			// This flag should always be true. 
			bool flag = intersect_face_face (mesh1.faces[in_map_iter->first], mesh2.faces[in_map_iter->second], intrRes, coplanar) ;

			if (flag)
			{
				if (!coplanar)
				{
					if (intrRes.res_type == SEGMENT)
					{
						v_segment.push_back(intrRes.segment) ;
					}
					else if (intrRes.res_type == POINT)
					{
						std::cout<<"Point type intersection";
					}
					else
					{
						throw ("Cannot handle polygon. 1");
					}
				}
				else
				{
					if (intrRes.res_type == POLYGON)
					{
						unsigned size = intrRes.polygon.size();
						for (unsigned i = 0; i < size; ++i)
						{
							SegmentType segment ; 
							segment = SegmentType (intrRes.polygon[i], intrRes.polygon[(i + 1)% size]) ;
							v_segment.push_back(segment);
						}
					}
					else
					{
						//throw ("Cannot handle polygon. 2");
					}
				}
			}
			else
			{
				std::cout<<"id pair "<<id1<<' '<<id2<<std::endl; throw ("Inconsitency on predicator.");
			}
		}
/*
		// sort the face-face intersection
		std::cout<<"\nCut lines begin"<<std::endl;		
		for (unsigned i = 0; i < cut_line_set.size(); ++i)
		{
			
			for (unsigned j = 0; j < cut_line_set[i].size(); ++j)
			{
				std::cout<<cut_line_set[i][j] <<' ';
			}
			std::cout<<" HE ";
			std::cout<< cut_he_set[i].first <<' '
					 << cut_he_set[i].second ;
						
			std::cout<<std::endl;		   
		}
		std::cout<<"Cut lines end "<<std::endl;			
		merge_cut_lines (cut_line_set, cut_he_set);
		std::cout<<"\nNew Cut lines begin"<<std::endl;		
		for (unsigned i = 0; i < cut_line_set.size(); ++i)
		{
			
			for (unsigned j = 0; j < cut_line_set[i].size(); ++j)
			{
				std::cout<<cut_line_set[i][j] <<' ';
			}
			std::cout<<" HE ";
			std::cout<< cut_he_set[i].first <<' '
					 << cut_he_set[i].second ;
						
			std::cout<<std::endl;		   
		}
		std::cout<<"New Cut lines end "<<std::endl;
		cut_line_type.resize(cut_line_set.size()); // shrink the cut_line_type; 
		for (unsigned i = 0; i < cut_line_type.size(); ++i) cut_line_type[i] = true; 
*/
/*
		// sort the segment intersection
		std::vector<std::deque<Vector <ValueType> > > v_curves; 
		std::vector<std::deque<HalfEdgeType * > > v_hes; 
		std::vector<std::deque<SegmentStruct * > > v_segRecord; 
		populate_segment_intersect (split_face->get_polygon(), v_segment, v_halfedge, v_curves, v_hes, v_segRecord);

		// Adapter from v_curves to cutline ;
		for (unsigned i = 0; i< v_curves.size(); ++i)
		{
			std::vector<Vector <ValueType> > v (v_curves[i].begin(), v_curves[i].end());
			cut_line_type.push_back(false);
			cut_line_set.push_back (v);
			cut_he_set.push_back (std::make_pair(v_hes[i][0], v_hes[i][ v_hes[i].size() -1]));
		}
*/
		
		//build_faces(split_face , cut_line_set, cut_he_set, cut_line_type,  v_segRecord);		
		//static unsigned max_id = mesh1.vertices.size(); 
		
		FaceGraph <ValueType> fg(&mesh1, &mesh2, split_face, max_id); 

		/*
		for (unsigned i = 0; i < cut_line_set.size(); ++i)
		{
			for (unsigned j = 1; j < cut_line_set[i].size(); ++j)
			{
				fg.add_inside_segment(SegmentType (cut_line_set[i][j-1], cut_line_set[i][j] ));
			}
		}
		*/
		for (unsigned i = 0; i < v_segment.size(); ++i)
		{
			fg.add_inside_segment (v_segment[i]);
		}
		fg.add_boundary_segment(); 
		fg.build_graph();

		fg.build_face(*this, this->pt_type );
		max_id = fg.get_max_id(); 
	}


	/**
	 * Split the mesh along the curve. 
	 * @param filter_coplanar. True, filter the coplanar face; False, otherwise. 
	 */
	
	void split_mesh (MeshType & mesh1, MeshType & mesh2)
	{
		this->max_id = mesh1.vertices.size(); 
		typename std::multimap<unsigned, unsigned>::iterator map_iter, in_map_iter, key_begin_iter;
		if (this->map_mesh1to2.size() == 0) return ;

		map_iter = this->map_mesh1to2.begin() ; 
		key_begin_iter = map_iter ;
		unsigned key = map_iter->first; 
		++map_iter ;

		// Traversal the map in group with the same key
		for (; map_iter != this->map_mesh1to2.end(); ++ map_iter)
		{
			if (map_iter->first != key)
			{
				face_splitter(key_begin_iter, map_iter, mesh1, mesh2);
				// Update the iterator
				key_begin_iter =  map_iter;
				key = map_iter->first; 
			}
		}
		// Deal with the last group
		face_splitter(key_begin_iter, map_iter,  mesh1,  mesh2);
	}

private :
	std::vector<std::pair<HalfEdgeType * , HalfEdgeType * > >  v_halfedge ;

	std::multimap <unsigned, unsigned> map_mesh1to2; 
	//std::multimap <unsigned, unsigned> map_mesh2to1;

 

	AABBTreePolyType * tree2 ;
	AABBTreePolyType * tree ;


public:
	AABBTreePolyType * get_tree ()
	{
		return this->tree; 
	}

	struct SegmentStruct
	{
		Segment <Value> segment ;
		std::pair <unsigned, unsigned> face_key; 
		unsigned count ;
		/**
		 * For sort segment 
		 */
		SegmentStruct() {acc[0]= acc[1] = false; }
		bool acc[2];
	};
	MeshType * p_mesh2 ;
	unsigned pt_type ;

	std::set<int> v_face_id;
	std::vector<int> v_new_face_id ;

	std::vector<SegmentType > v_segment ;
	//std::vector<SegmentStruct> v_segment ;
	std::vector<Polygon <Value> > v_polygon ;

	std::set <EdgeType *> v_edges ;

	// This structure is used for merging intersection. 
	std::map <std::pair <unsigned, unsigned>, IntersectionRecord<Value> > map_mergeInter;
	unsigned max_id ; 
};

	

#endif
